#include <stdio.h>
#include "xparameters.h"
#include "Xil_types.h"
#include "XSpips.h"


#define XSpiPs_SendByte(BaseAddress, Data) \
		XSpiPs_Out32((BaseAddress) + XSPIPS_TXD_OFFSET, (Data))


#define XSpiPs_RecvByte(BaseAddress) \
		(u8)XSpiPs_In32((BaseAddress) + XSPIPS_RXD_OFFSET)

#define TIMEOUT_HANDLER	(retVal=XST_FAILURE);

#define NREAD	(0x60)
#define NWRITE	(0x61)

#define SIO (0xF0)
#define STS (0xFE)
#define SPG (0xFF)

#define SPIF	(0x80)
#define RACK	(0x20)
#define RXRDY	(0x02)
#define TXRDY	(0x01)

int InitSPIDevice(XSpiPs *Spi_ptr, u16 DeviceId);
//int XSpiPs_SetSS_SL(XSpiPs *InstancePtr, u8 SlaveSel);
int XSpiPs_PT_SL(XSpiPs *InstancePtr, u8 *SendBufPtr, u8 *RecvBufPtr,
		u32 sendByteCount, u32 *pReqRetByteCount);
int readBCM5396(XSpiPs *Spi_ptr, u8 page, u8 offset, u8 *pBuffer);
int writeBCM5396(XSpiPs *Spi_ptr, u8 page, u8 offset, u8 *pBuffer);

int test_bcm5396() {
	int Status, k;
	u8 workBuf[10];

	u8 FiberworkBuf[10];
	u8 i;
	XSpiPs Spi;
	u32 Zynq_SPI_offset, Zynq_SPI_Reg;
	int delay_time;

	xil_printf("---Entering main---\n\r");
	Status = InitSPIDevice(&Spi, XPAR_PS7_SPI_0_DEVICE_ID);
	if (Status == 0) {
		xil_printf("InitSPIDevice PASSED\r\n");
	} else {
		xil_printf("InitSPIDevice FAILED\r\n");
	}
	workBuf[0] = 0xf0;
	workBuf[1] = 0x1;
	workBuf[2] = 0x0;

	FiberworkBuf[0] = 0xe1;
	FiberworkBuf[1] = 0x1;
	FiberworkBuf[2] = 0x0;
	for (i = 0x10; i <= 0x1f; i++) {
		if((i!=0x12)&&(i!=0x13))
		{
			Status = writeBCM5396(&Spi, i, 0x20, workBuf);
		}
	}

	Status = writeBCM5396(&Spi, 0x12, 0x20, FiberworkBuf);
	Status = writeBCM5396(&Spi, 0x13, 0x20, FiberworkBuf);

	///////////////////read spi reg of zynq////////////
	xil_printf("*****************Read SPI Reg of ZYNQ******************\r\n");
	for (Zynq_SPI_offset = 0; Zynq_SPI_offset <= 0x2c;) {
		Zynq_SPI_Reg = XSpiPs_ReadReg(Spi.Config.BaseAddress, Zynq_SPI_offset);
		xil_printf("SPI Read: offset = 0x%x,data = 0x%x\r\n", Zynq_SPI_offset,
				Zynq_SPI_Reg);
		Zynq_SPI_offset = Zynq_SPI_offset + 4;
	}

	Zynq_SPI_offset = 0xfc;
	Zynq_SPI_Reg = XSpiPs_ReadReg(Spi.Config.BaseAddress, Zynq_SPI_offset);
	xil_printf("SPI Read: offset = 0x%x,data = 0x%x\r\n", Zynq_SPI_offset,
			Zynq_SPI_Reg);
	printf("*******************************************************\r\n");

	xil_printf("*****************Read SPI Reg of 5396******************\r\n");
	xil_printf("---BMC Status Registers(PAGE 0x10-0x1F)---\n\r");
	for (i = 0x10; i <= 0x1f; i++) {
		Status = readBCM5396(&Spi, i, 0x28, workBuf);
		xil_printf("port=%d,offset=0x28,data=0x%x\n\r", (i - 0x10), workBuf[2]);

		xil_printf("port=%d,offset=0x29,data=0x%x\n\r", (i - 0x10), workBuf[3]);

		Status = readBCM5396(&Spi, i, 0xa, workBuf);
		xil_printf("port=%d,offset=0xa,data=0x%x\n\r", (i - 0x10), workBuf[2]);
		xil_printf("port=%d,offset=0xb,data=0x%x\n\r", (i - 0x10), workBuf[3]);

		Status = readBCM5396(&Spi, i, 0x0, workBuf);
		xil_printf("port=%d,offset=0x0,data=0x%x\n\r", (i - 0x10), workBuf[2]);
		xil_printf("port=%d,offset=0x1,data=0x%x\n\r", (i - 0x10), workBuf[3]);

		xil_printf("-----------------------------------\r\n");
	}

	xil_printf("---BMC Control Registers(PAGE 0x10-0x1F)---\n\r");
	for (i = 0x10; i <= 0x1f; i++) {
		Status = readBCM5396(&Spi, i, 0x20, workBuf);
		xil_printf("port=%d,offset=0x20,data=0x%x\n\r", (i - 0x10), workBuf[2]);

		xil_printf("port=%d,offset=0x21,data=0x%x\n\r", (i - 0x10), workBuf[3]);

		xil_printf("-----------------------------------\r\n");
	}

	xil_printf("-------LINK STATUS SUMMARY------\r\n");

	Status = readBCM5396(&Spi, 1, 0, workBuf);
	xil_printf("page= 1,offset=0,data=0x%x\n\r", workBuf[2]);
	xil_printf("page= 1,offset=1,data=0x%x\n\r", workBuf[3]);

	xil_printf("-------PORT SPEED SUMMARY------\r\n");

	Status = readBCM5396(&Spi, 1, 8, workBuf);
	xil_printf("page= 1,offset=8,data=0x%x\n\r", workBuf[2]);
	xil_printf("page= 1,offset=9,data=0x%x\n\r", workBuf[3]);
	xil_printf("page= 1,offset=0xa,data=0x%x\n\r", workBuf[4]);
	xil_printf("page= 1,offset=0xb,data=0x%x\n\r", workBuf[5]);

	xil_printf("-------DUPLEX STATUS SUMMARY------\r\n");

	Status = readBCM5396(&Spi, 1, 0x10, workBuf);
	xil_printf("page= 1,offset=0x10,data=0x%x\n\r", workBuf[2]);
	xil_printf("page= 1,offset=0x11,data=0x%x\n\r", workBuf[3]);

	xil_printf("-------PORT N PHY STATUS REGISTER------\r\n");

	for (i = 0x20; i <= 0x2f; i++) {
		Status = readBCM5396(&Spi, 1, i, workBuf);
		xil_printf("port=%d,offset=%x,data=0x%x\n\r", (i - 0x20), i,
				workBuf[2]);

	}

	xil_printf("-------PORT N STATE OVERRIDE REGISTER------\r\n");
	for (i = 0x60; i <= 0x6f; i++) {
		Status = readBCM5396(&Spi, 0, i, workBuf);
		xil_printf("port=%d,offset=%x,data=0x%x\n\r", (i - 0x60), i,
				workBuf[2]);

	}

	xil_printf("-------SERDES SIGNAL DETECT STATUS REGISTER------\r\n");
	Status = readBCM5396(&Spi, 1, 0x40, workBuf);
	xil_printf("page= 1,offset=0x40,data=0x%x\n\r", workBuf[2]);
	xil_printf("page= 1,offset=0x41,data=0x%x\n\r", workBuf[3]);

	xil_printf("-------BIST STATUS REGISTER------\r\n");
	Status = readBCM5396(&Spi, 1, 0x46, workBuf);
	xil_printf("page= 1,offset=0x46,data=0x%x\n\r", workBuf[2]);

	xil_printf("-------STRAP VALUE REGISTER------\r\n");

	Status = readBCM5396(&Spi, 1, 0x70, workBuf);
	xil_printf("page= 1,offset=0x70,data=0x%x\n\r", workBuf[2]);
	xil_printf("page= 1,offset=0x71,data=0x%x\n\r", workBuf[3]);

	Status = readBCM5396(&Spi, 1, 0x72, workBuf);
	xil_printf("page= 1,offset=0x72,data=0x%x\n\r", workBuf[2]);
	xil_printf("page= 1,offset=0x73,data=0x%x\n\r", workBuf[3]);

	xil_printf("*******************************************************\r\n");

	Status = readBCM5396(&Spi, 1, 0x40, workBuf);
	xil_printf("offset=0x40,data=0x%x\n\r", workBuf[2]);

	xil_printf("---Management Mode Registers(PAGE 2)---\n\r");

	Status = readBCM5396(&Spi, 2, 0x30, workBuf);
	xil_printf("offset=0x30,data=0x%x\n\r", workBuf[2]);

	xil_printf("*******************************************************\r\n");

	xil_printf("---Exiting main---\n\r");
	return 0;
}

int InitSPIDevice(XSpiPs *Spi_ptr, u16 DeviceId) {
	int i32Status, i32Option;
	XSpiPs_Config *SpiConfig;

	//Lookup the SPI device.
	SpiConfig = XSpiPs_LookupConfig(DeviceId);
	if (NULL == SpiConfig) {
		xil_printf("Call XSpiPs_LookupConfig Failed.\n\r");
		return XST_FAILURE;
	}

	//Initialize the SPI device.
	i32Status = XSpiPs_CfgInitialize(Spi_ptr, SpiConfig,
			SpiConfig->BaseAddress);
	if (i32Status != XST_SUCCESS) {
		return i32Status;
	}

//	{XSPIPS_MASTER_OPTION, XSPIPS_CR_MSTREN_MASK},
//	{XSPIPS_CLK_ACTIVE_LOW_OPTION, XSPIPS_CR_CPOL_MASK},
//	{XSPIPS_CLK_PHASE_1_OPTION, XSPIPS_CR_CPHA_MASK},
//	{XSPIPS_DECODE_SSELECT_OPTION, XSPIPS_CR_SSDECEN_MASK},
//	{XSPIPS_FORCE_SSELECT_OPTION, XSPIPS_CR_SSFORCE_MASK}
	i32Option = XSPIPS_MASTER_OPTION | XSPIPS_CLK_ACTIVE_LOW_OPTION
			| XSPIPS_CLK_PHASE_1_OPTION | XSPIPS_FORCE_SSELECT_OPTION;
	i32Status = XSpiPs_SetOptions(Spi_ptr, i32Option);
	if (i32Status != XST_SUCCESS) {
		return i32Status;
	}

	//i32Status = XSpiPs_SetSS_SL(Spi_ptr, 1);
	i32Status = XSpiPs_SetSlaveSelect(Spi_ptr, 1); //FLASH_SPI_SELECT
	if (i32Status != XST_SUCCESS) {
		return i32Status;
	}

	i32Status = XSpiPs_SetClkPrescaler(Spi_ptr, 6);
	if (i32Status != XST_SUCCESS) {
		return i32Status;
	}

	return XST_SUCCESS;
}

int XSpiPs_PT_SL(XSpiPs *InstancePtr, u8 *SendBufPtr, u8 *RecvBufPtr,
		u32 sendByteCount, u32 *pReqRetByteCount) {
	u32 StatusReg;
	u32 ControlReg;
	u8 TempData;

	// The RecvBufPtr argument can be NULL.
	Xil_AssertNonvoid(InstancePtr != NULL);
	Xil_AssertNonvoid(SendBufPtr != NULL);
	Xil_AssertNonvoid(sendByteCount > 0);
	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);

	// Check whether there is another transfer in progress. Not thread-safe.
	if (InstancePtr->IsBusy) {
		return XST_DEVICE_BUSY;
	}

	// Set the busy flag, which will be cleared in the ISR when the transfer is entirely done.
	InstancePtr->IsBusy = TRUE;

	// Set up buffer pointers.
	InstancePtr->SendBufferPtr = SendBufPtr;
	InstancePtr->RecvBufferPtr = RecvBufPtr;

	InstancePtr->RequestedBytes = *pReqRetByteCount;
	InstancePtr->RemainingBytes = sendByteCount + *pReqRetByteCount;

	//////////////////////////////////////////////////////////////////////////////
	// Enable the Manual Start EN bit in Control Register.
	ControlReg = XSpiPs_ReadReg(InstancePtr->Config.BaseAddress,
			XSPIPS_CR_OFFSET);
	ControlReg |= XSPIPS_CR_MANSTRTEN_MASK;
	XSpiPs_WriteReg(InstancePtr->Config.BaseAddress, XSPIPS_CR_OFFSET,
			ControlReg);

	// If the slave select lines are "Forced" or under manual control, set the slave selects now, before beginning the transfer.
	ControlReg = XSpiPs_ReadReg(InstancePtr->Config.BaseAddress,
			XSPIPS_CR_OFFSET);
	if (0 != (ControlReg & XSPIPS_CR_SSFORCE_MASK)) {
		ControlReg &= ~XSPIPS_CR_SSCTRL_MASK;
		ControlReg |= InstancePtr->SlaveSelect;

		XSpiPs_WriteReg(InstancePtr->Config.BaseAddress, XSPIPS_CR_OFFSET,
				ControlReg);
	}

	//////////////////////////////////////////////////////////////////////////////
	// Enable the device.
	XSpiPs_Enable(InstancePtr);

	while ((InstancePtr->RemainingBytes > 0))// || (InstancePtr->RequestedBytes > 0))
	{

		// Fill the DTR/FIFO with as many bytes as it will take (or as many as we have to send).
		while ((InstancePtr->RemainingBytes > 0)
				&& ((XSpiPs_ReadReg(InstancePtr->Config.BaseAddress,
						XSPIPS_SR_OFFSET)
						& XSPIPS_IXR_TXFULL_MASK) == 0)) {
			XSpiPs_SendByte(InstancePtr->Config.BaseAddress,
					*InstancePtr->SendBufferPtr);
			InstancePtr->SendBufferPtr++;
			InstancePtr->RemainingBytes--;
		}
		//////////////////////////////////////////////////////////////////////////////
		// If, in Manual Start mode, start the transfer.
		if ((ControlReg & (XSPIPS_CR_MSTREN_MASK | XSPIPS_CR_MANSTRTEN_MASK))
				== (XSPIPS_CR_MSTREN_MASK | XSPIPS_CR_MANSTRTEN_MASK)) {
			ControlReg |= XSPIPS_CR_MANSTRT_MASK;
			XSpiPs_WriteReg(InstancePtr->Config.BaseAddress, XSPIPS_CR_OFFSET,
					ControlReg);
		}

		// Wait for the transfer to finish by polling Tx fifo status.
		do {
			StatusReg = XSpiPs_ReadReg(InstancePtr->Config.BaseAddress,
					XSPIPS_SR_OFFSET);
		} while ((StatusReg & XSPIPS_IXR_TXOW_MASK) == 0);
		/*
		 * A transmit has just completed. Process received data
		 * and check for more data to transmit.
		 * First get the data received as a result of the
		 * transmit that just completed. We get all the data
		 * available by reading the status register to determine
		 * when the Receive register/FIFO is empty. Always get
		 * the received data, but only fill the receive
		 * buffer if it points to something (the upper layer
		 * software may not care to receive data).
		 */
		while ((StatusReg & XSPIPS_IXR_RXNEMPTY_MASK) != 0) {
			TempData = XSpiPs_RecvByte(InstancePtr->Config.BaseAddress);
			if (InstancePtr->RecvBufferPtr != NULL) {
				*InstancePtr->RecvBufferPtr++ = (u8) TempData;
			}
			InstancePtr->RequestedBytes--;

			// This barrier is placed to overcome the clock domain crossing issue in controller which does not update the status quick enough.
			dmb();
			StatusReg = XSpiPs_ReadReg(InstancePtr->Config.BaseAddress,
					XSPIPS_SR_OFFSET);
		}
	}

	*pReqRetByteCount = *pReqRetByteCount - InstancePtr->RequestedBytes;

	//////////////////////////////////////////////////////////////////////////////
	// Transfer has finished, now disable transmitter.
	ControlReg = XSpiPs_ReadReg(InstancePtr->Config.BaseAddress,
			XSPIPS_CR_OFFSET);
	ControlReg &= ~XSPIPS_CR_MANSTRT_MASK;
//	ControlReg |= XSPIPS_CR_SSCTRL_MASK;
	XSpiPs_WriteReg(InstancePtr->Config.BaseAddress, XSPIPS_CR_OFFSET,
			ControlReg);

	InstancePtr->IsBusy = FALSE;

	//////////////////////////////////////////////////////////////////////////////
	// Disable the device.
	XSpiPs_Disable(InstancePtr);

	//////////////////////////////////////////////////////////////////////////////
	// If the slave select lines are "Forced" or under manual control, set the slave selects now, before beginning the transfer.
	ControlReg = XSpiPs_ReadReg(InstancePtr->Config.BaseAddress,
			XSPIPS_CR_OFFSET);
	if (0 != (ControlReg & XSPIPS_CR_SSFORCE_MASK)) {
		ControlReg |= XSPIPS_CR_SSCTRL_MASK;

		XSpiPs_WriteReg(InstancePtr->Config.BaseAddress, XSPIPS_CR_OFFSET,
				ControlReg);
	}

	return XST_SUCCESS;
}

void delay_SL(u32 delayCount) {
	do {
		__asm__("nop");
		delayCount--;
	} while (delayCount > 0);
}

int readBCM5396(XSpiPs *Spi_ptr, u8 page, u8 offset, u8 *pBuffer) {
//	_BCM_CMD_ bcmCmd;
	u8 data[8];	//, u8Idx;
	s32 retVal;
	u32 u32SendNum, u32ReqRetNum;

	//
//	bcmCmd.pData = data;

	// Set Page
	/*
	 bcmCmd.cmd = NWRITE;
	 bcmCmd.regAdrs = SPG;
	 data[0] = page;
	 */
	data[0] = NWRITE;
	data[1] = SPG;
	data[2] = page;

	u32SendNum = 3;
	u32ReqRetNum = 0;
	//retVal = XSpiPs_PT_SL( Spi_ptr, data, pBuffer, u32SendNum, &u32ReqRetNum );
	retVal = XSpiPs_PolledTransfer(Spi_ptr, data, pBuffer, u32SendNum);
	if (retVal != XST_SUCCESS) {
		xil_printf("Call XSpiPs_PT_SL 1 Failed\n\r");
	}

	// Read STS
	READ_STS_1:
	/*
	 bcmCmd.cmd = NREAD;
	 bcmCmd.regAdrs = STS;
	 */
	data[0] = NREAD;
	data[1] = STS;
	u32SendNum = 2;
	u32ReqRetNum = 1;
	//retVal = XSpiPs_PT_SL(Spi_ptr, data, pBuffer, u32SendNum, &u32ReqRetNum );
	retVal = XSpiPs_PolledTransfer(Spi_ptr, data, pBuffer,
			u32SendNum + u32ReqRetNum);
	/*
	 xil_printf("STS 1 [%d]\n\r", retVal );
	 for( u8Idx=0; u8Idx<u32ReqRetNum; u8Idx++ )
	 xil_printf("%02x ", pBuffer[u8Idx] );
	 xil_printf("\n\r");
	 */
	//
	if (retVal == XST_SUCCESS) {
		if ((pBuffer[2] & SPIF) == 0) {
			// Set Page
			/*
			 bcmCmd.cmd = NWRITE;
			 bcmCmd.regAdrs = SPG;
			 data[0] = page;
			 */
			data[0] = NWRITE;
			data[1] = SPG;
			data[2] = page;
			u32SendNum = 3;
			u32ReqRetNum = 0;
			//retVal = XSpiPs_PT_SL(Spi_ptr, data, pBuffer, u32SendNum, &u32ReqRetNum );
			retVal = XSpiPs_PolledTransfer(Spi_ptr, data, pBuffer,
					u32SendNum + u32ReqRetNum);
			if (retVal != XST_SUCCESS) {
				xil_printf("Call XSpiPs_PT_SL 1 Failed\n\r");
			}

			// Set Offset in Page, "Null" operation :)
			/*
			 bcmCmd.cmd = NREAD;
			 bcmCmd.regAdrs = offset;
			 */
			data[0] = NREAD;
			data[1] = offset;
			u32SendNum = 2;
			u32ReqRetNum = 1;
			//retVal = XSpiPs_PT_SL(Spi_ptr, data, pBuffer, u32SendNum, &u32ReqRetNum );
			retVal = XSpiPs_PolledTransfer(Spi_ptr, data, pBuffer,
					u32SendNum + u32ReqRetNum);
			if (retVal != XST_SUCCESS) {
				xil_printf("Call XSpiPs_PT_SL 2 Failed\n\r");
			}

			// Read STS
			READ_STS_2:
			/*
			 bcmCmd.cmd = NREAD;
			 bcmCmd.regAdrs = STS;
			 */
			data[0] = NREAD;
			data[1] = STS;
			u32SendNum = 2;
			u32ReqRetNum = 1;
			//retVal = XSpiPs_PT_SL(Spi_ptr, data, pBuffer, u32SendNum, &u32ReqRetNum );
			retVal = XSpiPs_PolledTransfer(Spi_ptr, data, pBuffer,
					u32SendNum + u32ReqRetNum);
			/*
			 xil_printf("STS 2 [%d]\n\r", retVal );
			 for( u8Idx=0; u8Idx<u32ReqRetNum; u8Idx++ )
			 xil_printf("%02x ", pBuffer[u8Idx] );
			 xil_printf("\n\r");
			 */
			if (retVal == XST_SUCCESS) {
				if (pBuffer[2] & RACK) {
					/*
					 bcmCmd.cmd = NREAD;
					 bcmCmd.regAdrs = SIO;
					 */
					data[0] = NREAD;
					data[1] = SIO;
					u32SendNum = 2;
					u32ReqRetNum = 4;
					//retVal = XSpiPs_PT_SL( Spi_ptr, data, pBuffer, u32SendNum, &u32ReqRetNum );
					retVal = XSpiPs_PolledTransfer(Spi_ptr, data, pBuffer,
							u32SendNum + u32ReqRetNum);
					/*
					 xil_printf("DATA [%d]\n\r", retVal );
					 for( u8Idx=0; u8Idx<u32ReqRetNum; u8Idx++ )
					 xil_printf("%02x ", pBuffer[u8Idx] );
					 xil_printf("\n\r");
					 */
				} else {
					TIMEOUT_HANDLER
					;
					xil_printf("Timeout 2 Occured!\n\r");
					delay_SL(0x100000);
					// Set Page
					/*
					 bcmCmd.cmd = NWRITE;
					 bcmCmd.regAdrs = SPG;
					 data[0] = page;
					 */
					data[0] = NWRITE;
					data[1] = SPG;
					data[2] = page;
					u32SendNum = 3;
					u32ReqRetNum = 0;
					//retVal = XSpiPs_PT_SL(Spi_ptr, data, pBuffer, u32SendNum, &u32ReqRetNum );
					retVal = XSpiPs_PolledTransfer(Spi_ptr, data, pBuffer,
							u32SendNum + u32ReqRetNum);
					if (retVal != XST_SUCCESS) {
						xil_printf("Call XSpiPs_PT_SL 1 Failed\n\r");
					}

					goto READ_STS_2;
				}

			} else
				xil_printf("Call XSpiPs_PT_SL 3 Failed\n\r");
		} else {
			TIMEOUT_HANDLER
			;
			xil_printf("Timeout 1 Occured!\n\r");
			delay_SL(0x100000);

			// Set Page
			/*
			 bcmCmd.cmd = NWRITE;
			 bcmCmd.regAdrs = SPG;
			 data[0] = page;
			 */
			data[0] = NWRITE;
			data[1] = SPG;
			data[2] = page;
			u32SendNum = 3;
			u32ReqRetNum = 0;
			//retVal = XSpiPs_PT_SL(Spi_ptr, data, pBuffer, u32SendNum, &u32ReqRetNum );
			retVal = XSpiPs_PolledTransfer(Spi_ptr, data, pBuffer,
					u32SendNum + u32ReqRetNum);
			if (retVal != XST_SUCCESS) {
				xil_printf("Call XSpiPs_PT_SL 1 Failed\n\r");
			}

			goto READ_STS_1;
		}
	} else
		xil_printf("Call XSpiPs_PT_SL 4 Failed\n\r");

	return retVal;
}

int writeBCM5396(XSpiPs *Spi_ptr, u8 page, u8 offset, u8 *pBuffer) {
	u8 data[20];
	s32 retVal;
	u32 u32SendNum, u32ReqRetNum;

	int i;
	for (i = 0; i < 20; i++)
		data[i] = i;
	// Set Page
	data[0] = NWRITE;
	data[1] = SPG;
	data[2] = page;
	u32SendNum = 3;
	u32ReqRetNum = 0;
//	retVal = XSpiPs_PT_SL(Spi_ptr , data, pBuffer, u32SendNum, &u32ReqRetNum );
	retVal = XSpiPs_PolledTransfer(Spi_ptr, data, NULL, u32SendNum);//pBuffer
	if (retVal != XST_SUCCESS) {
		xil_printf("Call XSpiPs_PT_SL 1 Failed\n\r");
	}

	// Read STS
	READ_STS_1: data[0] = NREAD;
	data[1] = STS;
	u32SendNum = 2;
	u32ReqRetNum = 1;
//	retVal = XSpiPs_PT_SL(Spi_ptr , data, pBuffer, u32SendNum, &u32ReqRetNum );
	retVal = XSpiPs_PolledTransfer(Spi_ptr, data, NULL,
			u32SendNum + u32ReqRetNum);
	/*
	 xil_printf("STS 1 [%d]\n\r", retVal );
	 for( u8Idx=0; u8Idx<u32ReqRetNum; u8Idx++ )
	 xil_printf("%02x ", workBuf[u8Idx] );
	 xil_printf("\n\r");
	 */
	//
	if (retVal == XST_SUCCESS) {
		if ((pBuffer[2] & SPIF) == 0)	//( workBuf[2] & SPIF )
				{
			// Set Page
			data[0] = NWRITE;
			data[1] = SPG;
			data[2] = page;
			u32SendNum = 3;
			u32ReqRetNum = 0;
			//	retVal = XSpiPs_PT_SL(Spi_ptr , data, pBuffer, u32SendNum, &u32ReqRetNum );
			retVal = XSpiPs_PolledTransfer(Spi_ptr, data, NULL,
					u32SendNum + u32ReqRetNum);
			if (retVal != XST_SUCCESS) {
				xil_printf("Call XSpiPs_PT_SL 1 Failed\n\r");
			}

			// Write Data
			data[0] = NWRITE;
			data[1] = offset;
			data[2] = pBuffer[0];
			data[3] = pBuffer[1];
			u32SendNum = 4;
			u32ReqRetNum = 0;	//1;
			//retVal = XSpiPs_PT_SL(Spi_ptr , data, pBuffer, u32SendNum, &u32ReqRetNum );
			retVal = XSpiPs_PolledTransfer(Spi_ptr, data, NULL,
					u32SendNum + u32ReqRetNum);
			if (retVal != XST_SUCCESS) {
				xil_printf("Call XSpiPs_PT_SL 2 Failed\n\r");
			}

		} else {
			TIMEOUT_HANDLER
			;
			xil_printf("Timeout 1 Occured!\n\r");
			delay_SL(0x100000);

			// Set Page
			data[0] = NWRITE;
			data[1] = SPG;
			data[2] = page;
			u32SendNum = 3;
			u32ReqRetNum = 0;
			//retVal = XSpiPs_PT_SL(Spi_ptr , data, pBuffer, u32SendNum, &u32ReqRetNum );
			retVal = XSpiPs_PolledTransfer(Spi_ptr, data, pBuffer,
					u32SendNum + u32ReqRetNum);
			if (retVal != XST_SUCCESS) {
				xil_printf("Call XSpiPs_PT_SL 1 Failed\n\r");
			}

			goto READ_STS_1;
		}
	} else
		xil_printf("Call XSpiPs_PT_SL 4 Failed\n\r");

	return retVal;
}

